# -*- coding: utf-8 -*-
"""
Created on Fri Mar 20 17:02:03 2015

@author: QS
"""

from pylab import *

def ecef2enu(pos,ecef):        
    Lambda=pos['lon']*pi/180
    Theta=pos['lat']*pi/180
    h=pos['hei']
    
    RE_WGS84=6378137.0           #earth semimajor axis (WGS84) (m) =a 长半径
    FE_WGS84=(1.0/298.257223563) #earth flattening (WGS84) =(a-b)/a
    e2=FE_WGS84*(2.0-FE_WGS84)
    N=RE_WGS84/sqrt(1.0-e2*sin(Theta)**2)
    
    x,y,z=((N+h)*cos(Theta)*cos(Lambda), (N+h)*cos(Theta)*sin(Lambda), (N*(1-e2)+h)*sin(Theta))
    S=mat([[-sin(Lambda),           cos(Lambda),                    0.0],
           [-sin(Theta)*cos(Lambda),-sin(Theta)*sin(Lambda), cos(Theta)],
           [cos(Theta)*cos(Lambda), cos(Theta)*sin(Lambda), sin(Theta)]])
    return S*mat([[ecef[0]-x],[ecef[1]-y],[ecef[2]-z]])

print ecef2enu({'lon':113.392899274,'lat':23.037453729,'hei':48.7102},[-2331594.1343,5389830.3799,2480556.7292])